import java.awt.Color;
import java.awt.Dimension;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.GridLayout;
import java.util.ArrayList;

import javax.swing.BorderFactory;
import javax.swing.JPanel;


public class RobotPanel extends JPanel{

	ArrayList<Robot> robots;
	ArrayList<Light> lights;
	
	public RobotPanel(){
		
//		this.setLayout(new GridLayout(10,2));
		this.setBorder(BorderFactory.createLineBorder(Color.black));
		this.setSize(new Dimension(800,600));
		this.setPreferredSize(new Dimension(800,600));
		this.setVisible(true);
		
		robots = new ArrayList<Robot>();
		lights = new ArrayList<Light>();
	}

	public void paintComponent(Graphics g){
		super.paintComponent(g);
		Graphics2D g2d = (Graphics2D) g;
		
		g2d.setColor(Color.black);
		
		for(Robot rob: robots){
			g2d.rotate(-rob.getTheta(),(rob.getPosX())+10+rob.getWidth()/2,(rob.getPosY()+10)+rob.getHeight()/2);
			g2d.fillRect(rob.getPosX()+10, rob.getPosY()+10, rob.getWidth(), rob.getHeight());
			g2d.setColor(Color.green);
			g2d.fillRect(rob.getPosX()+15, rob.getPosY()+10+rob.getHeight(), rob.getWidth()/5, rob.getHeight()/5);
			g2d.fillRect(rob.getPosX()+15, rob.getPosY()+6, rob.getWidth()/5, rob.getHeight()/5);
			g2d.setColor(Color.red);
			g2d.fillOval(rob.getPosX()+rob.getWidth()+8, rob.getPosY()+rob.getHeight(), 9, 9);
			g2d.fillOval(rob.getPosX()+rob.getWidth()+8, rob.getPosY()+10, 9, 9);
	//		g2d.setColor(Color.white);
//			g2d.fillRect(rob.getPosX()+rob.getWidth()+18, rob.getPosY()+rob.getHeight()+2, 6,6);
			g2d.setColor(Color.black);
			
			g2d.rotate(rob.getTheta(),(rob.getPosX())+10+rob.getWidth()/2,(rob.getPosY())+10+rob.getHeight()/2);
		}
		
		g2d.setColor(Color.yellow);
		for(Light light: lights){
			g2d.fillOval(light.getPosX(), light.getPosY(), 10, 10);
		}
	}
	
	public void addRobot(Robot rob)
	{
		robots.add(rob);
	}
	
	public void addLight(Light light)
	{
		lights.add(light);
	}
	
	public void clear()
	{
		lights.clear();
		robots.clear();
		repaint();
	}
	
	// find dist b/t rob and light
	// TODO:would probably be better to find distance from a sensor...
	public double distRobotLight(Robot rob, Light light)
	{
		double dist;
		dist = Math.sqrt(Math.pow(light.getPosX()-rob.s1.getPosX(), 2) + Math.pow(light.getPosY()-rob.s1.getPosY(), 2));
		return dist;
	}
	double s=0;
	int op=0;
	public void updateRobots()
	{
		//System.out.println("here");
		if (lights.isEmpty() || robots.isEmpty())
			return;
		
		Light minDistLight = new Light(999,999,0);
		double dist = 9999999;
		double mindist = dist;
		for (Robot rob : robots) {
			// find the closest light for the robot
			for (Light light: lights) {
				dist = distRobotLight(rob, light);
				if (dist < mindist)
				{
					minDistLight = light;
					mindist = dist;
				}
			}
			
			// set speed and new position
			rob.setSpeed(minDistLight);
			rob.calcLocation();
			
			// reset stuff
			dist = 9999999;
			mindist = dist;
			

		}

		repaint();
	}
}
